/**
 * @file ekf_robot.cpp
 * @author Linfu Wei (ghowoght@qq.com)
 * @brief ADIS16465 + 里程计 测试
 * @version 1.0
 * @date 2022-03-21
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#include "ekf.hpp"
#include "mec.hpp"
#include "nav_base.hpp"
#include <fstream>
#include <iomanip>
#include <iostream>

#define D2R 3.1415926535 / 180.0

int main(int argc, char **argv) {

    auto startTime  = 182815.0;
    auto endTime    = 184758.0;

    ImuModel imumodel;
    imumodel.ARW                  = 0.001 / 60.0 * D2R;  // deg/sqrt(hr)
    imumodel.VRW                  = 0.1 / 60.0;        // m/s/sqrt(hr)
    imumodel.gyro_bias_std        = 10 * D2R / 3600.0; // deg/hr
    imumodel.gyro_bias_corr_time  = 1 * 3600.0;
    imumodel.accel_bias_std       = 50 * 1e-5; // mGal 1mGal=1e-5Gal
    imumodel.accel_bias_corr_time = 1 * 3600.0;
    imumodel.odom_var             = 1.2; // odom观测噪声方差
    imumodel.odom_scale_std       = 10000; // odom比例因子标准差

    StatePtr state = std::make_shared<State>();

    state->accel_bias << 104.4e-5, -111.74e-5, 314.06e-5;
    state->gyro_bias << 3.4479949e-4, -1.734033e-04, 8.97390124e-05;
    state->odom_scale_factor = 0;
    state->pos << 0, 0, 0;
    state->vel << 0, 0, 0;
    state->att << 0, 0.109 * D2R, -0.23 * D2R;
    state->imu_mount_angle << -0.31 * D2R, -0.3 * D2R, 0; // yaw, pitch, roll
    state->odom_lever_arm << 0, 0, 1.099;

    ImuData imudata;

    EKF ekf;

    DataPtr  data_raw = std::make_shared<Data>();
    OdomData odomdata;

    std::ifstream data_fs(
        "/home/ghowoght/workspace/ekf/data/HL_INSPROBE_9_VEL_IMU_ODO.txt");
    data_fs     >> imudata.timestamp
                >> imudata.gyro_rps[0] >> imudata.gyro_rps[1] >> imudata.gyro_rps[2]
                >> imudata.accel_mpss[0] >> imudata.accel_mpss[1] >> imudata.accel_mpss[2]
                >> odomdata.vel[0] >> odomdata.vel[1];
    odomdata.vel[0] = (odomdata.vel[0] + odomdata.vel[1]) / 2.0;
    state->timestamp = odomdata.timestamp = imudata.timestamp;
    odomdata.vel[1] = odomdata.vel[2] = 0;
    odomdata.valid                    = true;
    data_raw->imudata                 = imudata;
    data_raw->odomdata                = odomdata;
    
    state->timestamp = startTime;

    ekf.initialize(state, data_raw, imumodel);
    while (!data_fs.eof()){
        data_fs     >> imudata.timestamp
                    >> imudata.gyro_rps[0] >> imudata.gyro_rps[1] >> imudata.gyro_rps[2]
                    >> imudata.accel_mpss[0] >> imudata.accel_mpss[1] >> imudata.accel_mpss[2]
                    >> odomdata.vel[0] >> odomdata.vel[1];
        odomdata.vel[0] = (odomdata.vel[0] + odomdata.vel[1]) / 2.0;
        odomdata.timestamp = imudata.timestamp;
        odomdata.vel[1] = odomdata.vel[2] = 0;
        odomdata.valid                    = true;
        data_raw->imudata                 = imudata;
        data_raw->odomdata                = odomdata;

        if(imudata.timestamp > startTime && imudata.timestamp < endTime)
            ekf.Filter(data_raw);
    }
}